package Behaviors.Schemas.Perception;
import Robot.*;
import Robot.Sensors.Sensor;
import Util.Vector2d;
import Util.Pose2d;


public class PSDetectObstaclesVirtualObj extends PSDetectObstacles {

    private Vector2d[] virtualObjCenter;
    private Position2d[] obstacles;
    private float radius;
    private Position2d myPos;
    
    public PSDetectObstaclesVirtualObj(String name,Position2d myPos,Vector2d[] virtualObjCenter, float radius) {
        super(name);
        this.virtualObjCenter = virtualObjCenter;
        this.radius = radius ;
        this.myPos = myPos;


        
    }
    public PSDetectObstaclesVirtualObj(String name,Position2d myPos,Position2d[] obstacles, float radius) {
        super(name);
        this.obstacles = obstacles;
        this.radius = radius;
        this.myPos = myPos;
    }

    
    public Vector2d[] Value() {
        
        //if the obstacles were passed as Position2d types, then extract its Vector2d
        if(obstacles!=null)
        {
            virtualObjCenter = new Vector2d[obstacles.length];
            for(int x=0;x<obstacles.length;x++)
            {
                obstacles[x].read(Sensor.READ_MODE_NO_WAIT);
                virtualObjCenter[x] = obstacles[x].getPositionVector();
                
  
            }
        }
        //extract  the real position of obstacles, taking the nearest point of the obstacle to the position of robot
        Vector2d[] value = new Vector2d[virtualObjCenter.length];
        for(int i= 0;i<value.length;i++)
        {
            //calculate vector from obstacle to position of robot
            value[i] = myPos.getPositionVector().sub(virtualObjCenter[i]);
            //define size of object
            value[i].setMag(radius);
            //define final position of object
            value[i] = value[i].add(virtualObjCenter[i]);
            /*
            System.out.print("x");
            System.out.print(i);
            System.out.print(": ");

            System.out.print(value[i].getX());
            System.out.print(" y");
            System.out.print(i);
            System.out.print(": ");

            System.out.println(value[i].getY());
            */
            
        }
        return value;
    }
}